//------------------------------------------------------------------------------
//  <copyright file="GeometricCalculations.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Geometry
{
    using System;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// A class that consists of basic geometric calculations
    /// that can be shared across different classes.
    /// </summary>
    public static class GeometricCalculations
    {
        /// <summary>
        /// Compute the distances between the surface of two spheres.
        /// Positive distance means they do not collide.
        /// Negative distance evaluates the penetration depth.
        /// </summary>
        /// <param name="e1">One sphere</param>
        /// <param name="e2">Another sphere</param>
        /// <returns>Distance between the two spheres</returns>
        public static double ComputeDistance(SphereElement e1, SphereElement e2)
        {
            // first transform e2 into e1's coordinate system
            Matrix4 element2Ine1World = HomogenousTransform.Inverse(e1.ReferenceFrame) * e2.ReferenceFrame * HomogenousTransform.CreateFromTranslation(e2.Location);
            Vector3 element2LocationTransformedToe1World = new Vector3(element2Ine1World[0, 3], element2Ine1World[1, 3], element2Ine1World[2, 3]);

            // do checking in e1's world
            return (e1.Location - element2LocationTransformedToe1World).Length() - (e1.Radius + e2.Radius);
        }

        /// <summary>
        /// Compute the distances between a triangle and a sphere
        /// The side of the triangle is not considered.  So, there is no
        /// difference whether the sphere is in the back or front of the
        /// triangle.
        /// If the return value is positive, there is no collision.
        /// If the return value is negative, it indicates the penetration depth.
        /// </summary>
        /// <param name="e1">The triangle</param>
        /// <param name="e2">The sphere</param>
        /// <returns>Distance between the triangle and the sphere</returns>
        public static double ComputeDistance(TriangleElement e1, SphereElement e2)
        {
            // first transform e2 into e1's coordinate system
            Matrix4 element2Ine1World = HomogenousTransform.Inverse(e1.ReferenceFrame) * e2.ReferenceFrame * HomogenousTransform.CreateFromTranslation(e2.Location);
            Vector3 element2LocationTransformedToe1World = new Vector3(element2Ine1World[0, 3], element2Ine1World[1, 3], element2Ine1World[2, 3]);

            // step 1: decide which side of each edge of the triangle this sphere is on
            Vector3 dp0p1 = Vector3.Normalize(e1.V1 - e1.V0);
            Vector3 dp1p2 = Vector3.Normalize(e1.V2 - e1.V1);
            Vector3 dp2p0 = Vector3.Normalize(e1.V0 - e1.V2);

            Vector3 dp0ps = Vector3.Normalize(element2LocationTransformedToe1World - e1.V0);
            Vector3 dp1ps = Vector3.Normalize(element2LocationTransformedToe1World - e1.V1);
            Vector3 dp2ps = Vector3.Normalize(element2LocationTransformedToe1World - e1.V2);

            // decide which side of each of the three edges the projection of the sphere.
            // assuming we are facing the triangle from its positive side (which is defined
            // by its normal.
            // if a dot product is positive, then on the right side of the corresponding edge
            // because the cross product result is along the same direction as the normal of
            // the triangle.
            double sidep0p1 = Vector3.Dot(Vector3.Cross(dp0ps, dp0p1), e1.Normal);
            double sidep1p2 = Vector3.Dot(Vector3.Cross(dp1ps, dp1p2), e1.Normal);
            double sidep2p0 = Vector3.Dot(Vector3.Cross(dp2ps, dp2p0), e1.Normal);

            // on the left side of p0p1
            if (sidep0p1 < 0)
            {
                return (ProjectPointOntoLineSegment(e1.V0, e1.V1, element2LocationTransformedToe1World) - element2LocationTransformedToe1World).Length() - e2.Radius;
            }

            // on the left side of p1p2
            if (sidep1p2 < 0)
            {
                return (ProjectPointOntoLineSegment(e1.V1, e1.V2, element2LocationTransformedToe1World) - element2LocationTransformedToe1World).Length() - e2.Radius;
            }

            // on the left side of p2p0
            if (sidep2p0 < 0)
            {
                return (ProjectPointOntoLineSegment(e1.V2, e1.V0, element2LocationTransformedToe1World) - element2LocationTransformedToe1World).Length() - e2.Radius;
            }

            // within the triangle
            Vector3 p0ps = element2LocationTransformedToe1World - e1.V0;
            double cosine = Vector3.Dot(p0ps, e1.Normal) / p0ps.Length();
            Vector3 projected = element2LocationTransformedToe1World - p0ps.Length() * cosine * e1.Normal;

            return (element2LocationTransformedToe1World - projected).Length() - e2.Radius;
        }

        /// <summary>
        /// Computes the distance from a 3D point to a triangle
        /// Description of this implementation can be seen here $\\robofile\Baker\Shared\Users\bpeasley\DistancePoint3Triangle3.pdf$
        /// </summary>
        /// <param name="triangle">The triangle</param>
        /// <param name="point">The 3D points</param>
        /// <returns>The distance from the point to the triangle and whether or not that point lies inside the triangle when projected onto the plane formed by the triangles normal</returns>
        public static Tuple<double, bool> ComputeDistance(TriangleElement triangle, Vector3 point)
        {
            Vector3 line = triangle.V0 - point;
            Vector3 e0 = triangle.V1 - triangle.V0;
            Vector3 e1 = triangle.V2 - triangle.V0;

            double a = Vector3.Dot(e0, e0);
            double b = Vector3.Dot(e0, e1);
            double c = Vector3.Dot(e1, e1);
            double d = Vector3.Dot(e0, line);
            double e = Vector3.Dot(e1, line);

            double det = a * c - b * b;
            double s = b * e - c * d;
            double t = b * d - a * e;

            bool liesInTriangle = false;

            if (s + t <= det)
            {
                if (s < 0)
                {
                    if (t < 0)
                    {
                        // Region 4
                        s = 0;
                        t = 0;
                    }
                    else
                    {
                        // Region 3
                        s = 0;
                        t = Math.Min(1, -e / c);
                    }
                }
                else if (t < 0)
                {
                    // Region 5
                    t = 0;
                    s = Math.Min(1, -d / a);
                }
                else
                {
                    // Region 0
                    double invDet = 1 / det;
                    s *= invDet;
                    t *= invDet;
                    liesInTriangle = true;
                }
            }
            else
            {
                if (s < 0)
                {
                    // Region 2
                    s = 0;
                    t = 1;
                }
                else if (t < 0)
                {
                    // Region 6
                    t = 0;
                    s = 1;
                }
                else
                {
                    // Region 1
                    double numer = c + e - b - d;
                    if (numer <= 0)
                    {
                        s = 0;
                    }
                    else
                    {
                        double denom = a - 2 * b + c;
                        s = numer >= denom ? 1 : numer / denom;
                    }

                    t = 1 - s;
                }
            }

            Vector3 closestPoint = triangle.V0 + e0 * s + e1 * t;

            return new Tuple<double, bool>((point - closestPoint).Length(), liesInTriangle);
        }

        /// <summary>
        /// Computes the $barycentric$ coordinates of the point with respect to a triangle
        /// </summary>
        /// <param name="triangle">The triangle</param>
        /// <param name="point">The point</param>
        /// <returns>The $barycentric$ coordinates</returns>
        public static Vector3 Barycentric(TriangleElement triangle, Vector3 point)
        {
            Vector3 v0 = triangle.V1 - triangle.V0;
            Vector3 v1 = triangle.V2 - triangle.V0;
            Vector3 v2 = point - triangle.V0;

            double d00 = Vector3.Dot(v0, v0);
            double d01 = Vector3.Dot(v0, v1);
            double d11 = Vector3.Dot(v1, v1);
            double d20 = Vector3.Dot(v2, v0);
            double d21 = Vector3.Dot(v2, v1);

            double denom = d00 * d11 - d01 * d01;
            double v = (d11 * d20 - d01 * d21) / denom;
            double w = (d00 * d21 - d01 * d20) / denom;
            double u = 1.0 - v - w;

            return new Vector3(u, v, w);
        }

        /// <summary>
        /// Computes the intersection point of a ray with a triangle
        /// </summary>
        /// <param name="triangle">The triangle</param>
        /// <param name="ray">The ray</param>
        /// <param name="origin">The originating point of the ray</param>
        /// <returns>A Tuple containing the distance and intersection point.  Null value means no intersection</returns>
        public static Tuple<double, Vector3> RayTriangleInstersection(TriangleElement triangle, Vector3 ray, Vector3 origin)
        {
            Tuple<double, Vector3> intersection = GeometricCalculations.RayPlaneIntersection(triangle.Normal, triangle.V0, ray, origin);
            if (intersection != null)
            {
                Vector3 barycentric = GeometricCalculations.Barycentric(triangle, intersection.Item2);
                if (barycentric.X >= 0 && barycentric.X <= 1 && barycentric.Y >= 0 && barycentric.Y <= 1 && barycentric.Z >= 0 && barycentric.Z <= 1)
                {
                    return intersection;
                }
            }

            return null;
        }

        /// <summary>
        /// Computes the intersection of a ray with a plane
        /// </summary>
        /// <param name="normal">The normal that defines the plane</param>
        /// <param name="planePoint">Some point on the plane</param>
        /// <param name="ray">The ray, must be unit vector (not checked for performance)</param>
        /// <param name="origin">The originating point of the ray</param>
        /// <returns>A Tuple containing the distance and intersection point.  Null value means no intersection</returns>
        public static Tuple<double, Vector3> RayPlaneIntersection(Vector3 normal, Vector3 planePoint, Vector3 ray, Vector3 origin)
        {
            Vector3 w = planePoint - origin;
            double denom = Vector3.Dot(normal, ray);
            if (denom != 0)
            {
                double distance = (normal.X * w.X + normal.Y * w.Y + normal.Z * w.Z) / (Vector3.Dot(normal, ray));
                return new Tuple<double, Vector3>(distance, origin + ray * distance);
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Compute the projection of a given point onto a line segment bounded by the two end points
        /// of the line segment.  If the projection is outside the line segment, the closest end point
        /// of the line segment will be returned.
        /// </summary>
        /// <param name="lineEndPoint1">One end of the line segment</param>
        /// <param name="lineEndPoint2">Another end of the line segment</param>
        /// <param name="point">A point to be projected on to the line segment</param>
        /// <returns>The projection of the point on the line segment</returns>
        public static Vector3 ProjectPointOntoLineSegment(Vector3 lineEndPoint1, Vector3 lineEndPoint2, Vector3 point)
        {
            Vector3 lp1ToPoint = point - lineEndPoint1;
            Vector3 lp2ToPoint = point - lineEndPoint2;
            Vector3 lp1Tolp2 = lineEndPoint2 - lineEndPoint1;

            if (Vector3.Dot(lp1Tolp2, lp1ToPoint) < 0)
            {
                // close to endpoiont1
                return lineEndPoint1;
            }
            else
            {
                if (Vector3.Dot(-lp1Tolp2, lp2ToPoint) < 0)
                {
                    // close to endpoint2
                    return lineEndPoint2;
                }
                else
                {
                    // project onto the line segment
                    Vector3 projectOntoTheLineSegment = lineEndPoint1 + Vector3.Dot(lp1Tolp2, lp1ToPoint) * Vector3.Normalize(lp1Tolp2) / lp1Tolp2.Length();
                    return projectOntoTheLineSegment;
                }
            }
        }
    }
}
